#include "utils.h"

GLvoid utils::relRotat(GLfloat angle, GLfloat x, GLfloat y, GLfloat z, GLfloat p[]){
	double cosine = cos(angle*M_PI/180);
	double sine = sin(angle*M_PI/180);
	GLfloat m11, m12, m13, m21, m22, m23, m31, m32 ,m33;
	GLfloat vx, vy, vz;
	GLfloat X, Y, Z;

	m11 = x*x + cosine*(1 - x*x);
	m12 = x*y*(1-cosine) - z*sine;
	m13 = z*x*(1-cosine) + y*sine;

	m21 = x*y*(1-cosine) + z*sine;
	m22 = y*y + cosine*(1 - y*y);
	m23 = y*z*(1-cosine) - x*sine;

	m31 = x*z*(1-cosine) - y*sine;
	m32 = y*z*(1-cosine) + x*sine;
	m33 = z*z + cosine*(1 - z*z);

	vx = p[0];
	vy = p[1];
	vz = p[2];

	X = vx*m11 + vy*m12 + vz*m13;
	Y = vx*m21 + vy*m22 + vz*m23;
	Z = vx*m31 + vy*m32 + vz*m33;

	p[0] = X;
	p[1] = Y;
	p[2] = Z;
}

GLvoid
utils::normalize(GLfloat v[])
{
    GLfloat l;

    //assert(v);

    l = (GLfloat)sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
    v[0] /= l;
    v[1] /= l;
    v[2] /= l;
}
/** @brief ~utils
  *
  * @todo: document this function
  */
 utils::~utils()
{

}

/** @brief utils
  *
  * @todo: document this function
  */
 utils::utils()
{

}
